#include <iostream>
#include "Eigen/Dense"
#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include<vector>

using namespace Eigen;
using Eigen::MatrixXd;
using namespace std;
const double pi = acos(-1.0);

class kinematics
{
private:
	private:

	Matrix4d T01, T12, T23, T34, T45, T56, T67, Tp, T03, Tr, T07, T;
	vector<double>theta_Fine;
	int L1 = 436, L3 = 217, L = 20;
	float L2 = 45.5, Ldo = 26.5;
	double line_coef_for = 4.5;//4.01;
	double line_coef_back = 4.5;//4.33;
	double m1, m2, m3, m4, m5, m6, m_dj_1, m_dj_2, m_dj_3, m_dj_4, h;
	double a = 0, b = 0, H1, H2, x, y, z, X, Y, Z, U1, U2, e;
	double m2_up, m2_down, m2_m, error, res_up, res_down, res_m;
	double a1, a2, c;
	double r, r1, r2, r3, r4, R = 14, l1, l2, l3, l4, l10 = 24 * 7, l20 = 24 * 7, l30 = 24 * 7, l40 = 24 * 7, Ldd = 12;
	double Line_1, Line_2, Line_3, Line_4, Line = 24;
	double j_m3, ja, jb;
	template<typename T>
	T Power(T x)
	{
		return x * x;
	}

	double g1(double x)
	{
		double a = 0;
		for (int i = 1; i < 8; i++)
		{
			a += 2 * 20 * cos((x) / 2) * sin(((x) / 2) + (i - 1) * (x));
		}
		a += sin(7 * x) * 45.5;
		return a;
	}

	double g2(double x)
	{
		double a = 0;
		for (int i = 1; i < 8; i++)
		{
			a += 2 * 20 * cos((x) / 2) * cos(((x) / 2) + (i - 1) * (x));
		}
		a += cos(7 * x) * 45.5;
		return a;
	}

	double jie(double x1, double x2, double x3, double x4, double x5)
	{
		/*double j_m3 = acos(x5 / x1);
		double ja = sin(j_m3) * x1;
		double jb = x2 + 436;
		double o1 = pi - asin(x3 / sqrt(ja * ja + jb * jb)) - asin(jb / (sqrt(ja * ja + jb * jb)));
		double h = 238 - 49.07 / tan(pi / 2 - o1);
		double b = x1 * x1 + (x2 + 436) * (x2 + 436) - x3 * x3 - (x4 - h) * (x4 - h) - x5 * x5;*/
		double b = x1 * x1 + (x2 + 436) * (x2 + 436) - x3 * x3 - x4 * x4 - x5 * x5;
		return b;
	}

	double jie__addh(double x1, double x2, double x3, double x4, double x5,int log)
	{
		double j_m3 = acos(x5 / x1);
		double ja = sin(j_m3) * x1;
		double jb = x2 + 436;
		double o1 = pi - asin(x3 / sqrt(ja * ja + jb * jb)) - asin(jb / (sqrt(ja * ja + jb * jb)));
		/*double h = 238 - 49.07 / tan(pi / 2 - o1);*/
		double h = 0;
		if (log == 1 || log == 6) {
			h = 238 - 49.07 / tan(pi / 2 - o1);
		}
		else if (log == 2 || log == 7) {
			h = 238 - 52/ tan(pi / 2 - o1);
		}
		else {
			h = 238 - 50.59 / tan(pi / 2 - o1);
		}
		double b = x1 * x1 + (x2 + 436) * (x2 + 436) - x3 * x3 - (x4 - h) * (x4 - h) - x5 * x5;
		/*double b = x1 * x1 + (x2 + 436) * (x2 + 436) - x3 * x3 - x4 * x4 - x5 * x5;*/
		return b;
	}

	std::vector<double> new_slo_0420(double m5, double m6, const Matrix4d& T,int log,int screw)
	{

		/*MatrixXd T_theta(10, 1);*/
		std::vector<double> T_theta(11);

		T07 = T;
		T56 << cos(m6), -sin(m6), 0, 0,
			0, 0, -1, 0,
			sin(m6), cos(m6), 0, 0,
			0, 0, 0, 1;

		Matrix4d T450, T451, T67N;
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T45 = T450 * T451;

		T67 << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;

		Tp = T07 * T67.inverse() * T56.inverse() * T45.inverse();
		double x = Tp(0, 3); double y = Tp(1, 3); double z = Tp(2, 3);
		double m2_up = 0.5;
		double m2_down = 0.1;
		double error = 0.000000001;

		double u4 = 0.5, u5 = 0, u6 = 0, u7 = 0, u8 = 0.001;
		while (abs((z / g1(u4))) >= 1 || abs((x / sqrt(u6 * u6 + u7 * u7))) >= 1) {
			u4 -= 0.0001;
			u5 = acos(z / g1(u4));
			u6 = sin(u5) * g1(u4);
			u7 = g2(u4) + 436;
		}
		m2_up = u4;
		while (abs((z / g1(u8))) >= 1 || abs((x / sqrt(u6 * u6 + u7 * u7))) >= 1) {
			u8 += 0.0001;
			u5 = acos(z / g1(u8));
			u6 = sin(u5) * g1(u8);
			u7 = g2(u8) + 436;
		}
		m2_down = u8;
		double res_up = jie(g1(m2_up), g2(m2_up), x, y, z);
		double res_down = jie(g1(m2_down), g2(m2_down), x, y, z);
		while (res_up * res_down < 0)
		{
			double m2_m = 0.5 * (m2_up + m2_down);
			double res_m = jie(g1(m2_m), g2(m2_m), x, y, z);
			if (res_m * res_down < 0)
			{
				m2_up = m2_m;
			}
			else
			{
				m2_down = m2_m;
			}
			if (fabs(m2_up - m2_down) < error)
			{
				break;
			}
		}
		double m2 = (m2_up + m2_down) / 2;
		//             m3 = acos(z / g1(m2));
		double m3 = acos(max(-1.0, min(1.0, z / g1(m2))));
		double a1 = g2(m2) + L1;
		double a2 = g1(m2) * sin(m3);
		double	m1 = asin((a1 * y + a2 * x) / (a1 * a1 + a2 * a2));
		/*double m1 = pi - asin(max(-1.0, min(1.0, x / sqrt(a2 * a2 + a1 * a1)))) - asin(max(-1.0, min(1.0, a1 / (sqrt(a2 * a2 + a1 * a1)))));
		double h = (238 - 49.07 / tan(pi / 2 - m1));*/

		T01 << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T12 << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;
		double X = (g1(m2) - sin(7 * m2) * 45.5) * cos(m3);
		double Y = (g1(m2) - sin(7 * m2) * 45.5) * sin(m3);
		double Z = (g2(m2) - cos(7 * m2) * 45.5);

		T23 << cos(m3) * cos(m3) * cos(7 * m2) + sin(m3) * sin(m3), cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;
		T03 = T01 * T12 * T23;
		Tr = T03.inverse() * T07 * T67.inverse();

		double m4 = asin(max(-1.0, min(1.0, -Tr(0, 2) / sin(m5))));

		double theta[6] = { m1, m2, m3, m4, m5, m6 };
		//��˿���͵����
		double r, r1, r2, r3, r4, l1, l2, l3, l4, m_dj_1, m_dj_2, m_dj_3, m_dj_4;
		int Ldd = 12;
		int R = 14;
		

		r = Ldd / (tan(m2 / 2));
		r1 = r - R * cos(m3 - pi/4);
		r2 = r - R * sin(m3 - pi/4);
		r3 = r + R * cos(m3 - pi/4);
		r4 = r + R * sin(m3 - pi/4);
		
		l1 = r1 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l2 = r2 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l3 = r3 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l4 = r4 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		
		m_dj_1 = (l1 - l10) / line_coef_for;
		m_dj_2 = (l2 - l20) / line_coef_for;
		m_dj_3 = (l3 - l30) / line_coef_back;
		m_dj_4 = (l4 - l40) / line_coef_back;

		//T_theta(0, 0) = m1; T_theta(1, 0) = m2; T_theta(2, 0) = m3;
		//T_theta(3, 0) = m4; T_theta(4, 0) = m5; T_theta(5, 0) = m6;
		//T_theta(6, 0) = m_dj_1; T_theta(7, 0) = m_dj_2; T_theta(8, 0) = m_dj_3;
		//T_theta(9, 0) = m_dj_4;
		T_theta[0] = m1; T_theta[1] = m2; T_theta[2] = m3;
		T_theta[3] = m4; T_theta[4] = m5; T_theta[5] = m6;
		T_theta[6] = m_dj_1; T_theta[7] = m_dj_2; T_theta[8] = m_dj_3;
		T_theta[9] = m_dj_4;

		if(log == 0 || log == 5)
		{
			//T_theta[10] = -8.17;
			T_theta[10] = -60;
		}else if(log == 3 && (screw==0 || screw==1))
		{
			T_theta[10] = 0;
		}else if((log == 3 && screw==2) || log == 8)
		{
			T_theta[10] = -48;
		}
		else if((log == 4 && (screw==2 || screw==3)) || log == 9)
		{
			T_theta[10] = -30;
			//T_theta[10] = -20;
		}
		else
		{
			T_theta[10] = 0;
		}
		
		return T_theta;
	}

	MatrixXd newk0116(double T_theta[])
	{
		Matrix4d u, A, B, C, D, E, F, G, H, T, E1, E2;		// Ed
		double a, b, h, X, Y, Z;

	/*	double m1 = T_theta(0, 0);
		double m2 = T_theta(1, 0);
		double m3 = T_theta(2, 0);
		double m4 = T_theta(3, 0);
		double m5 = T_theta(4, 0);
		double m6 = T_theta(5, 0);*/
		//cout << m2 << endl

		double m1 = T_theta[0];
		double m2 = T_theta[1];
		double m3 = T_theta[2];
		double m4 = T_theta[3];
		double m5 = T_theta[4];
		double m6 = T_theta[5];
		L1 = 436; L2 = 45.5; L3 = 217; L = 20; Ldo = 26.5;

		a = 0, b = 0;
		for (int i = 1; i < 8; i++) {
			a += 2 * 20 * cos(m2 / 2) * sin((m2 / 2) + (i - 1) * m2);
			b += 2 * 20 * cos(m2 / 2) * cos((m2 / 2) + (i - 1) * m2);
		}

		X = a * cos(m3);  Y = a * sin(m3); Z = b;
		//h = (238 - 49.07 / tan(pi / 2 - m1));

		//u << 1, 0, 0, 0,
		//	0, 1, 0, h,
		//	0, 0, 1, 0,
		//	0, 0, 0, 1;

		A << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;

		B << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;

		C << cos(m3) * cos(m3) * cos(7 * m2) + sin(m3) * sin(m3), cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;

		D << -sin(m4), -cos(m4), 0, 0,
			0, 0, -1, 0,
			cos(m4), -sin(m4), 0, L2,
			0, 0, 0, 1;
		E1 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		E2 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		E = E1 * E2;

		F << cos(m6), -sin(m6), 0, 0,
			0, 0, -1, 0,
			sin(m6), cos(m6), 0, 0,
			0, 0, 0, 1;

		G << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;

		// sunjian delete 2024-05-10 
		//Ed << -sin(m4) * Ldo, 0, cos(m4)* Ldo + L2, 1;

		/*H = u * A * B * C * D * E * F * G;*/
		H = A * B * C * D * E * F * G;
		T = H;



		return T;
	}

	std::vector<double> new_slo_0420__addh(double m5, double m6, const Matrix4d& T,int log)
	{

		/*MatrixXd T_theta(11, 1);*/
		std::vector<double> T_theta_addh(11);

		T07 = T;
		T56 << cos(m6), -sin(m6), 0, 0,
			0, 0, -1, 0,
			sin(m6), cos(m6), 0, 0,
			0, 0, 0, 1;

		Matrix4d T450, T451, T67N;
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T45 = T450 * T451;

		T67 << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;

		Tp = T07 * T67.inverse() * T56.inverse() * T45.inverse();
		double x = Tp(0, 3); double y = Tp(1, 3); double z = Tp(2, 3);
		double m2_up = 0.5;
		double m2_down = 0.1;
		double error = 0.000000001;

		double u4 = 0.5, u5 = 0, u6 = 0, u7 = 0, u8 = 0.001;
		while (abs((z / g1(u4))) >= 1 || abs((x / sqrt(u6 * u6 + u7 * u7))) >= 1) {
			u4 -= 0.0001;
			u5 = acos(z / g1(u4));
			u6 = sin(u5) * g1(u4);
			u7 = g2(u4) + 436;
		}
		m2_up = u4;
		while (abs((z / g1(u8))) >= 1 || abs((x / sqrt(u6 * u6 + u7 * u7))) >= 1) {
			u8 += 0.0001;
			u5 = acos(z / g1(u8));
			u6 = sin(u5) * g1(u8);
			u7 = g2(u8) + 436;
		}
		m2_down = u8;
		double res_up = jie__addh(g1(m2_up), g2(m2_up), x, y, z,log);
		double res_down = jie__addh(g1(m2_down), g2(m2_down), x, y, z,log);
		while (res_up * res_down < 0)
		{
			double m2_m = 0.5 * (m2_up + m2_down);
			double res_m = jie__addh(g1(m2_m), g2(m2_m), x, y, z,log);
			if (res_m * res_down < 0)
			{
				m2_up = m2_m;
			}
			else
			{
				m2_down = m2_m;
			}
			if (fabs(m2_up - m2_down) < error)
			{
				break;
			}
		}
		double m2 = (m2_up + m2_down) / 2;
		//             m3 = acos(z / g1(m2));
		double m3 = acos(max(-1.0, min(1.0, z / g1(m2))));
		double a1 = g2(m2) + L1;
		double a2 = g1(m2) * sin(m3);
		/*double	m1 = asin((a1 * y + a2 * x) / (a1 * a1 + a2 * a2));*/
		double m1 = pi - asin(max(-1.0, min(1.0, x / sqrt(a2 * a2 + a1 * a1)))) - asin(max(-1.0, min(1.0, a1 / (sqrt(a2 * a2 + a1 * a1)))));
		/*double h = (238 - 49.07 / tan(pi / 2 - m1));*/
		if (log == 1 || log == 6) {
			h = 238 - 49.07 / tan(pi / 2 - m1);
		}
		else if (log == 2 || log == 7) {
			h = 238 -52/ tan(pi / 2 - m1);
		}
		else {
			h = 238 - 50.59 / tan(pi / 2 - m1);
		}

		T01 << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T12 << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;
		double X = (g1(m2) - sin(7 * m2) * 45.5) * cos(m3);
		double Y = (g1(m2) - sin(7 * m2) * 45.5) * sin(m3);
		double Z = (g2(m2) - cos(7 * m2) * 45.5);

		T23 << cos(m3) * cos(m3) * cos(7 * m2) + sin(m3) * sin(m3), cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;
		T03 = T01 * T12 * T23;
		Tr = T03.inverse() * T07 * T67.inverse();

		double m4 = asin(max(-1.0, min(1.0, -Tr(0, 2) / sin(m5))));

		double theta[6] = { m1, m2, m3, m4, m5, m6 };
		
		double r, r1, r2, r3, r4, l1, l2, l3, l4, m_dj_1, m_dj_2, m_dj_3, m_dj_4;
		int Ldd = 12;
		int R = 14;
		
		r = Ldd / (tan(m2 / 2));
		r1 = r - R * cos(m3 - pi/4);
		r2 = r - R * sin(m3 - pi/4);
		r3 = r + R * cos(m3 - pi/4);
		r4 = r + R * sin(m3 - pi/4);

		l1 = r1 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l2 = r2 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l3 = r3 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		l4 = r4 * 2 * sin(m2 / 2) * 7 - ((pi/2)-m1)*10;
		
		m_dj_1 = (l1 - l10) / line_coef_for;
		m_dj_2 = (l2 - l20) / line_coef_for;
		m_dj_3 = (l3 - l30) / line_coef_back;
		m_dj_4 = (l4 - l40) / line_coef_back;
		/*T_theta(0, 0) = m1; T_theta(1, 0) = m2; T_theta(2, 0) = m3;
		T_theta(3, 0) = m4; T_theta(4, 0) = m5; T_theta(5, 0) = m6;
		T_theta(6, 0) = m_dj_1; T_theta(7, 0) = m_dj_2; T_theta(8, 0) = m_dj_3;
		T_theta(9, 0) = m_dj_4; T_theta(10, 0) =h;*/

		T_theta_addh[0] = m1; T_theta_addh[1] = m2; T_theta_addh[2] = m3;
		T_theta_addh[3] = m4; T_theta_addh[4] = m5; T_theta_addh[5] = m6;
		T_theta_addh[6] = m_dj_1; T_theta_addh[7] = m_dj_2; T_theta_addh[8] = m_dj_3;
		T_theta_addh[9] = m_dj_4; T_theta_addh[10] = h;
		/*return T_theta;*/
		return T_theta_addh;
	}
	MatrixXd jie_Matri_H(double m5,double m6, MatrixXd T07, int log,int screw)
	{
		vector<double> thetaNew, theta_test;
		MatrixXd T1, T_test;
		if (log == 1 || log == 6) {
			thetaNew = new_slo_0420__addh(m5, m6, T07, log);
			double* arr = thetaNew.data();
			T1 = newk0116__addh(arr, log);
		}
		else if (log == 2 || log == 7) {
			thetaNew = new_slo_0420__addh(m5, m6, T07, log);
			double* arr = thetaNew.data();
			T1 = newk0116__addh(arr, log);
		}
		else {
			theta_test = new_slo_0420(m5, m6, T07,log,screw);
			double* arr = theta_test.data();
			T1 = newk0116(arr);
		}
		return T1;
	}
	MatrixXd newk0116__addh(double T_theta[],int log)
	{
		Matrix4d u, A, B, C, D, E, F, G, H, T, E1, E2;		// Ed
		double a, b, h, X, Y, Z;

		/*double m1 = T_theta(0, 0);
		double m2 = T_theta(1, 0);
		double m3 = T_theta(2, 0);
		double m4 = T_theta(3, 0);
		double m5 = T_theta(4, 0);
		double m6 = T_theta(5, 0);*/
		//cout << m2 << endl;
		double m1 = T_theta[0];
		double m2 = T_theta[1];
		double m3 = T_theta[2]; 
		double m4 = T_theta[3];
		double m5 = T_theta[4];
		double m6 = T_theta[5];
		L1 = 436; L2 = 45.5; L3 = 217; L = 20; Ldo = 26.5;

		a = 0, b = 0;
		for (int i = 1; i < 8; i++) {
			a += 2 * 20 * cos(m2 / 2) * sin((m2 / 2) + (i - 1) * m2);
			b += 2 * 20 * cos(m2 / 2) * cos((m2 / 2) + (i - 1) * m2);
		}

		X = a * cos(m3);  Y = a * sin(m3); Z = b;
		/*h = (238 - 49.07 / tan(pi / 2 - m1));*/
		if (log == 1 || log == 6) {
			h = 238 - 49.07 / tan(pi / 2 - m1);
		}
		else if (log == 2 || log == 7) {
			h = 238 - 52/ tan(pi / 2 - m1);
		}
		else {
			h = 238 - 50.59 / tan(pi / 2 - m1);
		}
		u << 1, 0, 0, 0,
			0, 1, 0, h,
			0, 0, 1, 0,
			0, 0, 0, 1;

		A << cos(m1), -sin(m1), 0, 0,
			sin(m1), cos(m1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;

		B << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;

		C << cos(m3) * cos(m3) * cos(7 * m2) + sin(m3) * sin(m3), cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), cos(m3)* sin(7 * m2), X,
			cos(m3)* sin(m3)* cos(7 * m2) - cos(m3) * sin(m3), sin(m3)* sin(m3)* cos(7 * m2) + cos(m3) * cos(m3), sin(m3)* sin(7 * m2), Y,
			-cos(m3) * sin(7 * m2), -sin(m3) * sin(7 * m2), cos(7 * m2), Z,
			0, 0, 0, 1;

		D << -sin(m4), -cos(m4), 0, 0,
			0, 0, -1, 0,
			cos(m4), -sin(m4), 0, L2,
			0, 0, 0, 1;
		E1 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		E2 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		E = E1 * E2;

		F << cos(m6), -sin(m6), 0, 0,
			0, 0, -1, 0,
			sin(m6), cos(m6), 0, 0,
			0, 0, 0, 1;

		G << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;

		// sunjian delete 2024-05-10 
		//Ed << -sin(m4) * Ldo, 0, cos(m4)* Ldo + L2, 1;

		H = u * A * B * C * D * E * F * G;
		/*H = A * B * C * D * E * F * G;*/
		T = H;
		


		return T;
	}



	double rot2angle(Matrix3d& Q1, Matrix3d& Q2)
	{
		Eigen::Matrix3d R21 = Q1.transpose() * Q2;

		double trace_value = R21.trace();

		double theta_rad;

		if (std::abs(trace_value + 1.0) <= 1e-6) {
			theta_rad = 3.1415926; // 180 degrees  
		}
		else if (std::abs(trace_value - 3.0) <= 1e-6) {
			theta_rad = 0.0; // 0 degrees  
		}
		else {
			//trace(R) = 1 + 2*cos(theta)   
			theta_rad = std::acos((trace_value - 1.0) / 2.0);
		}
		return theta_rad;
	}




	void ShowData(MatrixXd& T, Matrix3d& Q1, double d[9])
	{ 
		MatrixXd subMatrix = T.block<3, 3>(0, 0);
		Q1 = subMatrix;
	}

	//void ShowData(MatrixXd& T, MatrixXd& R1)
	//{
	//	MatrixXd subMatrix = T.block<3, 3>(0, 0); 

	//	int index = 0;
	//	for (int i = 0; i < subMatrix.rows(); i++)
	//	{
	//		for (int j = 0; j < subMatrix.cols(); j++)
	//		{
	//			R1(i, j) = subMatrix(i, j);


	//		}
	//	}

	//}

	void Min_Matrix(Matrix3d& T, double& minvalue, int& minRow, int& minCol)
	{
		/*minvalue = T::numeric_limits<double>::max();*/
		minvalue = T(0, 0);
		for (int i = 0; i < 3; ++i) {
			for (int j = 0; j < 3; ++j) {
				if (T(i, j) < minvalue)
				{
					minvalue = T(i, j); 
					minRow = i;  
					minCol = j;
				}
			}
		}

	}

public:

	double theta_1, theta_2, theta_3, theta_4, theta_5, theta_6,theta5_for,theta6_for;
	double theta_z, theta_y, theta_x, Px, Py, Pz;



	std::vector<double> Inv_sol(int log, int screw)//逆解函数，输入三个欧拉角theta_z theta_y theta_x与三个位置Px Py Pz，输出向量前六项为m1—m6六个关节角及后四项为m_dj_1—m_dj_4四个线驱电机角度
	{
		double a = pi / 180;

		double theta5_init;
		double theta6_init;

		

		double theta_step = 0.01;
		double distance_test = 0.0;
		double zhuanjiao = 0.0;
		double posture_test = 0.0;
		double error_test = 0.0;
		double m6 = 0.0, m5 = 0.0;
		double M51 = 0.0, M61 = 0.0;
		double h = 0.0;

		/*double m1, m2, m3, m4;*/
		Matrix4d Tz, Tx, Ty;
		MatrixXd T07(4, 4), Tb, T, posture;
		double Distance_Fine1[200][200], Posture_Fine1[200][200];
		Matrix3d Q1, Q2, distance_Fine1, posture_Fine1, error1;
		map<int, MatrixXd> mvalue;
		vector<double>  Theta5, Theta6, M5_new, M6_new, M5Fine, M6Fine;
		/*VectorXd theta_Fine;*/
		MatrixXd distance;
		double R1[9];
		double R2[9];
		/*double theta_z, theta_y, theta_x, Px, Py, Pz;*/
		vector<double>theta_Fine;

      if (abs(theta5_for)<0.05*a)
		
		{
			theta5_init=90*a;
		}

		else
		{
			theta5_init=theta5_for;
		}

		 if (abs(theta6_for)<0.05*a)
		
		{
			theta6_init=0;
		}

		else
		{
			theta6_init=theta6_for;
		}
	//cout<<"theta 5 6  "<<theta5_init<<" "<<theta6_init<<endl;

		//double Px = v[i][0];
		//double Py = v[i][1];
		//double Pz = v[i][2];
		//double theta_x = v[i][3];
		//double theta_y = v[i][4];
		//double theta_z = v[i][5];


		

		Tz<<  cos(theta_z), -sin(theta_z), 0, 0,
			 sin(theta_z), cos(theta_z), 0 , 0,
			 0, 0, 1, 0,
			 0, 0, 0, 1;
		Ty<<  cos(theta_y), 0, sin(theta_y), 0,
			 0, 1, 0, 0,
			 -sin(theta_y), 0, cos(theta_y), 0,
			0, 0, 0, 1;
		Tx<< 1,0, 0, 0,
			 0, cos(theta_x), -sin(theta_x), 0,
			 0, sin(theta_x), cos(theta_x), 0,
			 0, 0, 0, 1;

		Tb = Tz * Ty * Tx;


		int j = 0;
		for (int mm = 0; mm < 3; mm++)
		{
			for (j = 0; j < 3; j++)
			{
				T07(mm, j) = Tb(mm, j);
			}
		}

		T07(0, 3) = Px;
		T07(1, 3) = Py;
		T07(2, 3) = Pz;
		T07(3, 3) = 1;
		T07(3, 0) = 0;
		T07(3, 1) = 0;
		T07(3, 2) = 0;


		/*T07 << 0.1903, 0.7908, - 0.5817, 488.1636,
			-0.9248, - 0.0544, - 0.3766, 533.8976,
			-0.3295, 0.6096, 0.7210, 182.5567,
			0, 0, 0, 1;*/

		T = T07;

		/*mvalue.insert(pair<int, MatrixXd>(i, T));*/
		/*VectorXd theta_test = new_slo_0420(theta5_init, theta6_init, T07);*/
		vector<double> theta_test, thetaNew;
		MatrixXd T_test,T1,Tnew,T_Fine;
		T_test = jie_Matri_H(theta5_init, theta6_init, T07, log,screw);

		distance_test = sqrt(Power(T_test(0, 3) - T07(0, 3)) + Power(T_test(1, 3) - T07(1, 3)) + Power(T_test(2, 3) - T07(2, 3)));
		ShowData(T07, Q1, R1);
		ShowData(T_test, Q2, R2);
		zhuanjiao = rot2angle(Q1, Q2);
		posture_test = abs(zhuanjiao);
		error_test = distance_test + 100 * posture_test;
		j = 0;
		int k = 0;
		Matrix3d P1, P2;
		double Distance[46][31], Posture[46][31];

		if (error_test > 10)
		{

			for (m6 = -45 * a; m6 < 45 * a; m6 += 2 * a)
			{
				Theta6.push_back(m6);

				k = 0;
				for (m5 = 60 * a; m5 < 120 * a; m5 += 2 * a)
				{

					
					T1 = jie_Matri_H(m5, m6, T07, log,screw);

					Theta5.push_back(m5);
					Distance[j][k] = sqrt(Power(T1(0, 3) - T07(0, 3)) + Power(T1(1, 3) - T07(1, 3)) + Power(T1(2, 3) - T07(2, 3)));

					ShowData(T07, P1, R1);
					ShowData(T1, P2, R2);
					zhuanjiao = rot2angle(P1, P2);
					Posture[j][k] = abs(zhuanjiao);
					k++;
				}

				j++;
			}

			int row = j;
			int col = k;

			double Distance_minvalue = Distance[0][0];
			double Posture_minvalue = Posture[0][0];
			int Distance_minRow = 0;
			int	Distance_minCol = 0;
			int Posture_minRow = 0;
			int Posture_minCol = 0;
			for (int a = 0;a < j;a++)
			{
				for (int b = 1; b < k; b++)
				{

					if (Distance[a][b] < Distance_minvalue)
					{
						Distance_minvalue = Distance[a][b];
						Distance_minRow = a;
						Distance_minCol = b;

					}
					if (Posture[a][b] < Posture_minvalue)
					{
						Posture_minvalue = Posture[a][b];
						Posture_minRow = a;
						Posture_minCol = b;
					}
				}
			}


			M5_new.push_back(Theta5.at(Distance_minCol) / a);

		
			Tnew = jie_Matri_H(Theta5.at(Posture_minCol), Theta6.at(Posture_minRow), T07, log,screw);


			double distance_error = sqrt(Power(T07(0, 3) - Tnew(0, 3)) + Power(T07(1, 3) - Tnew(1, 3)) + Power(T07(2, 3) - Tnew(2, 3)));

			if (distance_error >= 50)
			{

				double Posture_minvalue = Posture[0][Distance_minCol];
				int Posture_minrow = 0;
				for (int i = 1; i <= Posture_minRow; i++)
				{

					if (Posture[i][Distance_minCol] < Posture_minvalue)
					{
						Posture_minvalue = Posture[i][Distance_minCol];
						Posture_minrow = i;
					}
				}
				M6_new.push_back(Theta6.at(Posture_minrow) / a);
			}
			else
			{
				M6_new.push_back(Theta6.at(Posture_minRow) / a);
			}
			M51 = M5_new.at(0);
			M61 = M6_new.at(0);

			/*MatrixXd Temp;
			for (auto it = mvalue.begin(); it != mvalue.end(); ++it)
			{
				if (it->first == i)
				{
					Temp = it->second;
				}
			}*/
			/*vector<double>theta_Fine = new_slo_0420(M51 * a, M61 * a, T07);
			double* arr1 = theta_Fine.data();
			MatrixXd T_Fine = newk0116(arr1);*/
			
			T_Fine = jie_Matri_H(m5, m6, T07, log,screw);
			double distance_Fine = sqrt(Power(T_Fine(0, 3) - T07(0, 3)) + Power(T_Fine(1, 3) - T07(1, 3)) + Power(T_Fine(2, 3) - T07(2, 3)));
			ShowData(T07, Q1, R1);
			ShowData(T_Fine, Q2, R2);
			zhuanjiao = rot2angle(Q1, Q2);
			double posture_Fine = abs(zhuanjiao);
			double error = distance_Fine + 100 * posture_Fine;
			float epsion = 0.1;
			int caculate = 0;
			T = T07;


			while (error > epsion)
			{
				caculate++;

				std::vector<double> M511 = { M51 - theta_step, M51, M51 + theta_step };
				std::vector<double> M611 = { M61 - theta_step, M61, M61 + theta_step };
				for (int m = 0; m < 3; m++)
				{
					for (int n = 0; n < 3; n++)
					{
						
						T_Fine = jie_Matri_H(M511[m] * a, M611[n] * a, T, log,screw);
						distance_Fine1(m, n) = sqrt(Power(T_Fine(0, 3) - T(0, 3)) + Power(T_Fine(1, 3) - T(1, 3)) + Power(T_Fine(2, 3) - T(2, 3)));

						ShowData(T, Q1, R1);
						ShowData(T_Fine, Q2, R2);
						zhuanjiao = rot2angle(Q1, Q2);
						posture_Fine1(m, n) = abs(zhuanjiao);
						error1(m, n) = distance_Fine1(m, n) + 100 * posture_Fine1(m, n);
					}
				}
				double minvalue = 0;
				int error1_minRow = 0;
				int error1_minCol = 0;
				Min_Matrix(error1, minvalue, error1_minRow, error1_minCol);
				/*[minVal3, linearInd3] = min(error1(:));*/
				M51 = M511[error1_minRow];
				M61 = M611[error1_minCol];

				if (error1_minRow == 1 && error1_minCol == 1)
				{

					break;
				}

				if (caculate >= 1000)
				{

					break;
				}

			}

			theta5_init = M51 * a;
			theta6_init = M61 * a;

			
			if (log == 1 || log == 6) {
				thetaNew = new_slo_0420__addh(theta5_init, theta6_init, T07, log);
				double* arr = thetaNew.data();
				T1 = newk0116__addh(arr, log);
			}
			else if (log == 2 || log == 7) {
				thetaNew = new_slo_0420__addh(theta5_init, theta6_init, T07, log);
				double* arr = thetaNew.data();
				T1 = newk0116__addh(arr, log);
			}
			else {
				thetaNew = new_slo_0420(theta5_init, theta6_init, T07,log,screw);
				double* arr = thetaNew.data();
				T1 = newk0116(arr);
			}	

			T_Fine = jie_Matri_H(theta5_init, theta6_init, T07, log,screw);
			theta_Fine = thetaNew;
	/*		cout << T_Fine << endl;*/
			
			cout<<"粗搜索计算"<<endl;
		}

		else
		{
			/*MatrixXd Temp;
			for (auto it = mvalue.begin(); it != mvalue.end(); ++it)
			{
				if (it->first == i)
				{
					Temp = it->second;
				}
			}*/
			T = T07;
			M51 = theta5_init / a;
			M61 = theta6_init / a;

			
			T_Fine = jie_Matri_H(M51 * a, M61 * a, T, log,screw);
			double distance_Fine = sqrt(Power(T_Fine(0, 3) - T(0, 3)) + Power(T_Fine(1, 3) - T(1, 3)) + Power(T_Fine(2, 3) - T(2, 3)));

			ShowData(T, Q1, R1);
			ShowData(T_Fine, Q2, R2);
			zhuanjiao = rot2angle(Q1, Q2);
			double posture_Fine = abs(zhuanjiao);
			double error = distance_Fine + 100 * posture_Fine;
			double epsion = 0.1;
			int caculate = 0;
			
			while (error > epsion)
			{
				caculate++;

				std::vector<double> M511 = { M51 - theta_step, M51, M51 + theta_step };
				std::vector<double> M611 = { M61 - theta_step, M61, M61 + theta_step };
				for (int m = 0; m < 3; m++)
				{
					for (int n = 0; n < 3; n++)
					{
						
						T_Fine = jie_Matri_H(M511[m] * a, M611[n] * a, T, log,screw);
						distance_Fine1(m, n) = sqrt(Power(T_Fine(0, 3) - T(0, 3)) + Power(T_Fine(1, 3) - T(1, 3)) + Power(T_Fine(2, 3) - T(2, 3)));

						ShowData(T, Q1, R1);
						ShowData(T_Fine, Q2, R2);
						zhuanjiao = rot2angle(Q1, Q2);
						posture_Fine1(m, n) = abs(zhuanjiao);
						error1(m, n) = distance_Fine1(m, n) + 100 * posture_Fine1(m, n);
					}
				}
			
			
				double minvalue = 0;
				int error1_minRow = 0;
				int error1_minCol = 0;
				Min_Matrix(error1, minvalue, error1_minRow, error1_minCol);
				//[minVal3, linearInd3] = min(error1(:));
				M51 = M511[error1_minRow];
				M61 = M611[error1_minCol];
				/*cout << M51 << endl;
				cout << M51 << endl;*/
				if (error1_minRow == 1 && error1_minCol == 1)
				{

					break;
				}

				if (caculate >= 1000)
				{

					break;
				}

			}

             cout<< " 精细搜索次数  "<< caculate << endl;


			theta5_init = M51 * a;
			theta6_init = M61 * a;
			/* theta_Fine = new_slo_0420(theta5_init, theta6_init, T07);
			double* arr2 = theta_Fine.data();
			T_Fine = newk0116(arr2);*/
			T_Fine = jie_Matri_H(theta5_init, theta6_init, T07, log,screw);
			//cout << theta_Fine[0] << endl;
			/*cout << "�ڶ���ѭ��" << endl;
			cout << "px=" << T_Fine(0, 3) << "py=" << T_Fine(1, 3) << "pz=" << T_Fine(2, 3) << endl;*/
			if (log == 1 || log == 6) {
				thetaNew = new_slo_0420__addh(theta5_init, theta6_init, T07, log);
				double* arr = thetaNew.data();
				T1 = newk0116__addh(arr, log);
			}
			else if (log == 2 || log == 7) {
				thetaNew = new_slo_0420__addh(theta5_init, theta6_init, T07, log);
				double* arr = thetaNew.data();
				T1 = newk0116__addh(arr, log);
			}
			else {
				thetaNew = new_slo_0420(theta5_init, theta6_init, T07,log,screw);
				double* arr = thetaNew.data();
				T1 = newk0116(arr);
			}
			theta_Fine = thetaNew;
		} 
		return theta_Fine;
	}
	

	MatrixXd Jacobian_CamToBase()//输入六个角度theta_1-theta_6，输出雅可比逆解Tjac_Env(6,6)
	{
		MatrixXd Tjac_CamToBase_Env(8, 6), Tjac_CamToBase(6, 6);
		Tjac_CamToBase(0, 0) = cos(theta_1)*sin(theta_3)*(40*cos(theta_2/2)*sin(theta_2/2) + 40*cos(theta_2/2)*sin((3*theta_2)/2) + 40*cos(theta_2/2)*sin((5*theta_2)/2) + 40*cos(theta_2/2)*sin((7*theta_2)/2) + 40*cos(theta_2/2)*sin((9*theta_2)/2) + 40*cos(theta_2/2)*sin((11*theta_2)/2) + 40*cos(theta_2/2)*sin((13*theta_2)/2)) - sin(theta_1)*(40*cos(theta_2/2)*cos(theta_2/2) + 40*cos(theta_2/2)*cos((3*theta_2)/2) + 40*cos(theta_2/2)*cos((5*theta_2)/2) + 40*cos(theta_2/2)*cos((7*theta_2)/2) + 
                                                 40*cos(theta_2/2)*cos((9*theta_2)/2) + 40*cos(theta_2/2)*cos((11*theta_2)/2) + 40*cos(theta_2/2)*cos((13*theta_2)/2)) - (93*cos(7*theta_2)*sin(theta_1))/2 - (1007*cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)))/4 - (1007*sin(theta_5)*(cos(theta_4)*(cos(7*theta_2)*sin(theta_1) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3))
                                                - sin(7*theta_2)*cos(theta_3)*sin(theta_1))))/4 - 416*sin(theta_1) + (93*sin(7*theta_2)*cos(theta_1)*sin(theta_3))/2;

		Tjac_CamToBase(0, 1) = (1007*cos(theta_5)*(7*cos(7*theta_2)*cos(theta_1)*sin(theta_3) + 7*sin(7*theta_2)*sin(theta_1)*sin(theta_3)*sin(theta_3)))/4 - (651*sin(7*theta_2)*cos(theta_1))/2 - cos(theta_1)*(40*cos(theta_2/2)*sin(theta_2/2) + 60*cos(theta_2/2)*sin((3*theta_2)/2) + 20*cos((3*theta_2)/2)*sin(theta_2/2) + 100*cos(theta_2/2)*sin((5*theta_2)/2) + 20*cos((5*theta_2)/2)*sin(theta_2/2) + 140*cos(theta_2/2)*sin((7*theta_2)/2) + 20*cos((7*theta_2)/2)*sin(theta_2/2) + 180*cos(theta_2/2)*sin((9*theta_2)/2) 
                                                + 20*cos((9*theta_2)/2)*sin(theta_2/2) + 220*cos(theta_2/2)*sin((11*theta_2)/2) + 20*cos((11*theta_2)/2)*sin(theta_2/2) + 260*cos(theta_2/2)*sin((13*theta_2)/2) + 20*cos((13*theta_2)/2)*sin(theta_2/2)) - (1007*sin(theta_5)*(cos(theta_4)*(7*sin(7*theta_2)*cos(theta_1) - 7*cos(7*theta_2)*sin(theta_1)*sin(theta_3)) - sin(theta_4)*(7*cos(7*theta_2)*cos(theta_1)*cos(theta_3) + 7*sin(7*theta_2)*cos(theta_3)*sin(theta_1)*sin(theta_3))))/4 + sin(theta_1)*sin(theta_3)*(20*cos(theta_2/2)*cos(theta_2/2) - 20*sin(theta_2/2)*sin(theta_2/2) 
                                                + 60*cos(theta_2/2)*cos((3*theta_2)/2) + 100*cos(theta_2/2)*cos((5*theta_2)/2) + 140*cos(theta_2/2)*cos((7*theta_2)/2) + 180*cos(theta_2/2)*cos((9*theta_2)/2) + 220*cos(theta_2/2)*cos((11*theta_2)/2) + 260*cos(theta_2/2)*cos((13*theta_2)/2) - 20*sin(theta_2/2)*sin((3*theta_2)/2) - 20*sin(theta_2/2)*sin((5*theta_2)/2) - 20*sin(theta_2/2)*sin((7*theta_2)/2) - 20*sin(theta_2/2)*sin((9*theta_2)/2) - 20*sin(theta_2/2)*sin((11*theta_2)/2) - 20*sin(theta_2/2)*sin((13*theta_2)/2)) + (651*cos(7*theta_2)*sin(theta_1)*sin(theta_3))/2;
 

		Tjac_CamToBase(0, 2) =  (1007*sin(theta_5)*(sin(theta_4)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)) + sin(7*theta_2)*cos(theta_3)*cos(theta_4)*sin(theta_1)))/4 + (1007*cos(theta_5)*(sin(theta_1)*(2*cos(theta_3)*sin(theta_3) - 2*cos(7*theta_2)*cos(theta_3)*sin(theta_3)) + sin(7*theta_2)*cos(theta_1)*cos(theta_3)))/4 + cos(theta_3)*sin(theta_1)*(40*cos(theta_2/2)*sin(theta_2/2) + 40*cos(theta_2/2)*sin((3*theta_2)/2) 
                                               + 40*cos(theta_2/2)*sin((5*theta_2)/2) + 40*cos(theta_2/2)*sin((7*theta_2)/2) + 40*cos(theta_2/2)*sin((9*theta_2)/2) + 40*cos(theta_2/2)*sin((11*theta_2)/2) + 40*cos(theta_2/2)*sin((13*theta_2)/2)) + (93*sin(7*theta_2)*cos(theta_3)*sin(theta_1))/2;
 

		Tjac_CamToBase(0, 3) = -(1007*sin(theta_5)*(sin(theta_4)*(cos(7*theta_2)*cos(theta_1) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)) - cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)) + sin(7*theta_2)*cos(theta_1)*cos(theta_3))))/4;

		Tjac_CamToBase(0, 4) = (1007*sin(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)))/4 + (1007*cos(theta_5)*(cos(theta_4)*(cos(7*theta_2)*cos(theta_1) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)) + sin(7*theta_2)*cos(theta_1)*cos(theta_3))))/4;
 

		
                                Tjac_CamToBase(0, 5) =0;

		Tjac_CamToBase(1, 0) = 416*cos(theta_1) + (93*cos(7*theta_2)*cos(theta_1))/2 - (1007*cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)))/4 + (1007*sin(theta_5)*(cos(theta_4)*(cos(7*theta_2)*cos(theta_1) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)) 
                                               + sin(7*theta_2)*cos(theta_1)*cos(theta_3))))/4 + cos(theta_1)*(40*cos(theta_2/2)*cos(theta_2/2) + 40*cos(theta_2/2)*cos((3*theta_2)/2) + 40*cos(theta_2/2)*cos((5*theta_2)/2) + 40*cos(theta_2/2)*cos((7*theta_2)/2) + 40*cos(theta_2/2)*cos((9*theta_2)/2) + 40*cos(theta_2/2)*cos((11*theta_2)/2) + 40*cos(theta_2/2)*cos((13*theta_2)/2)) + sin(theta_1)*sin(theta_3)*(40*cos(theta_2/2)*sin(theta_2/2) 
                                               + 40*cos(theta_2/2)*sin((3*theta_2)/2) + 40*cos(theta_2/2)*sin((5*theta_2)/2) + 40*cos(theta_2/2)*sin((7*theta_2)/2) + 40*cos(theta_2/2)*sin((9*theta_2)/2) + 40*cos(theta_2/2)*sin((11*theta_2)/2) + 40*cos(theta_2/2)*sin((13*theta_2)/2)) + (93*sin(7*theta_2)*sin(theta_1)*sin(theta_3))/2;
 


		Tjac_CamToBase(1, 1) =   (1007*cos(theta_5)*(7*cos(7*theta_2)*sin(theta_1)*sin(theta_3) - 7*sin(7*theta_2)*cos(theta_1)*sin(theta_3)*sin(theta_3)))/4 - (651*sin(7*theta_2)*sin(theta_1))/2 - sin(theta_1)*(40*cos(theta_2/2)*sin(theta_2/2) + 60*cos(theta_2/2)*sin((3*theta_2)/2) + 20*cos((3*theta_2)/2)*sin(theta_2/2) + 100*cos(theta_2/2)*sin((5*theta_2)/2) + 20*cos((5*theta_2)/2)*sin(theta_2/2) 
                                              + 140*cos(theta_2/2)*sin((7*theta_2)/2) + 20*cos((7*theta_2)/2)*sin(theta_2/2) + 180*cos(theta_2/2)*sin((9*theta_2)/2) + 20*cos((9*theta_2)/2)*sin(theta_2/2) + 220*cos(theta_2/2)*sin((11*theta_2)/2) + 20*cos((11*theta_2)/2)*sin(theta_2/2) + 260*cos(theta_2/2)*sin((13*theta_2)/2) + 20*cos((13*theta_2)/2)*sin(theta_2/2)) - (1007*sin(theta_5)*(cos(theta_4)*(7*sin(7*theta_2)*sin(theta_1) 
                                              + 7*cos(7*theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(7*cos(7*theta_2)*cos(theta_3)*sin(theta_1) - 7*sin(7*theta_2)*cos(theta_1)*cos(theta_3)*sin(theta_3))))/4 - cos(theta_1)*sin(theta_3)*(20*cos(theta_2/2)*cos(theta_2/2) - 20*sin(theta_2/2)*sin(theta_2/2) + 60*cos(theta_2/2)*cos((3*theta_2)/2) + 100*cos(theta_2/2)*cos((5*theta_2)/2) + 140*cos(theta_2/2)*cos((7*theta_2)/2) + 180*cos(theta_2/2)*cos((9*theta_2)/2) 
                                              + 220*cos(theta_2/2)*cos((11*theta_2)/2) + 260*cos(theta_2/2)*cos((13*theta_2)/2) - 20*sin(theta_2/2)*sin((3*theta_2)/2) - 20*sin(theta_2/2)*sin((5*theta_2)/2) - 20*sin(theta_2/2)*sin((7*theta_2)/2) - 20*sin(theta_2/2)*sin((9*theta_2)/2) - 20*sin(theta_2/2)*sin((11*theta_2)/2) - 20*sin(theta_2/2)*sin((13*theta_2)/2)) - (651*cos(7*theta_2)*cos(theta_1)*sin(theta_3))/2;
 



		Tjac_CamToBase(1, 2) =  - (1007*sin(theta_5)*(sin(theta_4)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)) + sin(7*theta_2)*cos(theta_1)*cos(theta_3)*cos(theta_4)))/4 - (1007*cos(theta_5)*(cos(theta_1)*(2*cos(theta_3)*sin(theta_3) - 2*cos(7*theta_2)*cos(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_3)*sin(theta_1)))/4 
                                             - cos(theta_1)*cos(theta_3)*(40*cos(theta_2/2)*sin(theta_2/2) + 40*cos(theta_2/2)*sin((3*theta_2)/2) + 40*cos(theta_2/2)*sin((5*theta_2)/2) + 40*cos(theta_2/2)*sin((7*theta_2)/2) + 40*cos(theta_2/2)*sin((9*theta_2)/2) + 40*cos(theta_2/2)*sin((11*theta_2)/2) + 40*cos(theta_2/2)*sin((13*theta_2)/2)) - (93*sin(7*theta_2)*cos(theta_1)*cos(theta_3))/2;
 



		Tjac_CamToBase(1, 3) =-(1007*sin(theta_5)*(sin(theta_4)*(cos(7*theta_2)*sin(theta_1) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)) + cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_3)*sin(theta_1))))/4;



		Tjac_CamToBase(1, 4) = (1007*cos(theta_5)*(cos(theta_4)*(cos(7*theta_2)*sin(theta_1) - sin(7*theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)) - sin(7*theta_2)*cos(theta_3)*sin(theta_1))))/4 - (1007*sin(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)) + sin(7*theta_2)*sin(theta_1)*sin(theta_3)))/4;



		Tjac_CamToBase(1, 5) =0;


		Tjac_CamToBase(2, 0) = 0;


		Tjac_CamToBase(2, 1) =cos(theta_3)*(20*cos(theta_2/2)*cos(theta_2/2) - 20*sin(theta_2/2)*sin(theta_2/2) + 60*cos(theta_2/2)*cos((3*theta_2)/2) + 100*cos(theta_2/2)*cos((5*theta_2)/2) + 140*cos(theta_2/2)*cos((7*theta_2)/2) + 180*cos(theta_2/2)*cos((9*theta_2)/2) + 220*cos(theta_2/2)*cos((11*theta_2)/2) + 260*cos(theta_2/2)*cos((13*theta_2)/2) - 20*sin(theta_2/2)*sin((3*theta_2)/2) 
                                                    - 20*sin(theta_2/2)*sin((5*theta_2)/2) - 20*sin(theta_2/2)*sin((7*theta_2)/2) - 20*sin(theta_2/2)*sin((9*theta_2)/2) - 20*sin(theta_2/2)*sin((11*theta_2)/2) - 20*sin(theta_2/2)*sin((13*theta_2)/2)) + (651*cos(7*theta_2)*cos(theta_3))/2 + (1007*sin(theta_5)*(7*cos(7*theta_2)*cos(theta_3)*cos(theta_4) + 7*sin(7*theta_2)*cos(theta_3)*cos(theta_3)*sin(theta_4)))/4 + (7049*sin(7*theta_2)*cos(theta_3)*cos(theta_5)*sin(theta_3))/4;



		Tjac_CamToBase(2, 2) =(1007*cos(theta_5)*(cos(theta_3)*cos(theta_3) - sin(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*cos(theta_3) + cos(7*theta_2)*sin(theta_3)*sin(theta_3)))/4 - (93*sin(7*theta_2)*sin(theta_3))/2 - (1007*sin(theta_5)*(sin(theta_4)*(2*cos(theta_3)*sin(theta_3) - 2*cos(7*theta_2)*cos(theta_3)*sin(theta_3))
                                                 + sin(7*theta_2)*cos(theta_4)*sin(theta_3)))/4 - sin(theta_3)*(40*cos(theta_2/2)*sin(theta_2/2) + 40*cos(theta_2/2)*sin((3*theta_2)/2) + 40*cos(theta_2/2)*sin((5*theta_2)/2) + 40*cos(theta_2/2)*sin((7*theta_2)/2) + 40*cos(theta_2/2)*sin((9*theta_2)/2) + 40*cos(theta_2/2)*sin((11*theta_2)/2) + 40*cos(theta_2/2)*sin((13*theta_2)/2));



		Tjac_CamToBase(2, 3) =-(1007*sin(theta_5)*(cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7*theta_2)*cos(theta_3)*cos(theta_3)) + sin(7*theta_2)*cos(theta_3)*sin(theta_4)))/4;


		Tjac_CamToBase(2, 4) =- (1007*cos(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7*theta_2)*cos(theta_3)*cos(theta_3)) - sin(7*theta_2)*cos(theta_3)*cos(theta_4)))/4 - (1007*sin(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7*theta_2)*cos(theta_3)*sin(theta_3)))/4;



		Tjac_CamToBase(2, 5) =0;


		Tjac_CamToBase(3, 0) = 0;
		Tjac_CamToBase(3, 1) = 7 * cos(theta_3)*sin(theta_1);
		Tjac_CamToBase(3, 2) = cos(theta_1) - cos(7 * theta_2)*cos(theta_1) - sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(3, 3) = sin(7 * theta_2)*cos(theta_1)*sin(theta_3) - sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3));
		Tjac_CamToBase(3, 4) = cos(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3)) - sin(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3));
		Tjac_CamToBase(3, 5) = sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*cos(theta_1) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_4)*(sin(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) + sin(7 * theta_2)*cos(theta_1)*cos(theta_3))) - cos(theta_5)*(sin(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3));
		Tjac_CamToBase(4, 0) = 0;
		Tjac_CamToBase(4, 1) = -7 * cos(theta_1)*cos(theta_3);
		Tjac_CamToBase(4, 2) = sin(theta_1) - cos(7 * theta_2)*sin(theta_1) + sin(7 * theta_2)*cos(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 3) = cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3);
		Tjac_CamToBase(4, 4) = -sin(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - cos(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1));
		Tjac_CamToBase(4, 5) = cos(theta_5)*(cos(theta_1)*(cos(theta_3)*cos(theta_3) + cos(7 * theta_2)*sin(theta_3)*sin(theta_3)) + sin(7 * theta_2)*sin(theta_1)*sin(theta_3)) + sin(theta_5)*(cos(theta_4)*(cos(7 * theta_2)*sin(theta_1) - sin(7 * theta_2)*cos(theta_1)*sin(theta_3)) - sin(theta_4)*(cos(theta_1)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_1)));
		Tjac_CamToBase(5, 0) = 1;
		Tjac_CamToBase(5, 1) = -7 * sin(theta_3);
		Tjac_CamToBase(5, 2) = -sin(7 * theta_2)*cos(theta_3);
		Tjac_CamToBase(5, 3) = cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3);
		Tjac_CamToBase(5, 4) = -cos(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*sin(theta_4);
		Tjac_CamToBase(5, 5) = cos(theta_5)*(cos(theta_3)*sin(theta_3) - cos(7 * theta_2)*cos(theta_3)*sin(theta_3)) - sin(theta_5)*(sin(theta_4)*(sin(theta_3)*sin(theta_3) + cos(7 * theta_2)*cos(theta_3)*cos(theta_3)) - sin(7 * theta_2)*cos(theta_3)*cos(theta_4));
				
		Tjac_CamToBase_Env = Tjac_CamToBase.inverse();
		return Tjac_CamToBase_Env;
	}

	MatrixXd Get_Vel_Translation()
	{
		MatrixXd Tvel_CamToBase(6, 6);

		Tvel_CamToBase(0, 0) = cos(theta_6) * (sin(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3) + cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3))
			+ cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
				- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3)))) - sin(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * cos(theta_1)
					+ sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3))
						+ sin(7 * theta_2) * cos(theta_1) * cos(theta_3)));

		Tvel_CamToBase(0, 1) = sin(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))) - cos(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3));

		Tvel_CamToBase(0, 2) = cos(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))) + sin(theta_6) * (sin(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1)
					+ sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))));

		Tvel_CamToBase(0, 3) = 0;
		Tvel_CamToBase(0, 4) = 0;
		Tvel_CamToBase(0, 5) = 0;


		Tvel_CamToBase(1, 0) = -sin(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * sin(theta_1) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))) - cos(theta_6) * (sin(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1)
					- sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) - sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))));

		Tvel_CamToBase(1, 1) = cos(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3) + cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1)
			- sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) - sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1)));

		Tvel_CamToBase(1, 2) = cos(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * sin(theta_1) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))) - sin(theta_6) * (sin(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3))
					- sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))));

		Tvel_CamToBase(1, 3) = 0;
		Tvel_CamToBase(1, 4) = 0;
		Tvel_CamToBase(1, 5) = 0;

		Tvel_CamToBase(2, 0) = -sin(theta_6) * (cos(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) + sin(7 * theta_2) * cos(theta_3) * sin(theta_4)) - cos(theta_6) * (cos(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3)
			+ cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) - sin(7 * theta_2) * cos(theta_3) * cos(theta_4)) + sin(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)));
		Tvel_CamToBase(2, 1) = cos(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3))
			- sin(7 * theta_2) * cos(theta_3) * cos(theta_4));
		Tvel_CamToBase(2, 2) = cos(theta_6) * (cos(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) + sin(7 * theta_2) * cos(theta_3) * sin(theta_4)) - sin(theta_6) * (cos(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3)
			+ cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) - sin(7 * theta_2) * cos(theta_3) * cos(theta_4)) + sin(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)));

		Tvel_CamToBase(2, 3) = 0;
		Tvel_CamToBase(2, 4) = 0;
		Tvel_CamToBase(2, 5) = 0;

		Tvel_CamToBase(3, 0) = 0;
		Tvel_CamToBase(3, 1) = 0;
		Tvel_CamToBase(3, 2) = 0;

		Tvel_CamToBase(3, 3) = cos(theta_6) * (sin(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3) + cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3))
			+ cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
				- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3)))) - sin(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * cos(theta_1)
					+ sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3)));

		Tvel_CamToBase(3, 4) = sin(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))) - cos(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3) + cos(7 * theta_2) * sin(theta_3) * sin(theta_3))
				- sin(7 * theta_2) * cos(theta_1) * sin(theta_3));
		Tvel_CamToBase(3, 5) = cos(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * cos(theta_1) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))) + sin(theta_6) * (sin(theta_5) * (sin(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * cos(theta_1)
					+ sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_4) * (sin(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) + sin(7 * theta_2) * cos(theta_1) * cos(theta_3))));

		Tvel_CamToBase(4, 0) = 0;
		Tvel_CamToBase(4, 1) = 0;
		Tvel_CamToBase(4, 2) = 0;


		Tvel_CamToBase(4, 3) = -sin(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * sin(theta_1) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))) - cos(theta_6) * (sin(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1)
					- sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) - sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))));
		Tvel_CamToBase(4, 4) = cos(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3) + cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) + sin(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1)
			- sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) - sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1)));
		Tvel_CamToBase(4, 5) = cos(theta_6) * (sin(theta_4) * (cos(7 * theta_2) * sin(theta_1) - sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) + cos(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3)
			- cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))) - sin(theta_6) * (sin(theta_5) * (cos(theta_1) * (cos(theta_3) * cos(theta_3)
				+ cos(7 * theta_2) * sin(theta_3) * sin(theta_3)) + sin(7 * theta_2) * sin(theta_1) * sin(theta_3)) - cos(theta_5) * (cos(theta_4) * (cos(7 * theta_2) * sin(theta_1)
					- sin(7 * theta_2) * cos(theta_1) * sin(theta_3)) - sin(theta_4) * (cos(theta_1) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(7 * theta_2) * cos(theta_3) * sin(theta_1))));


		Tvel_CamToBase(5, 0) = 0;
		Tvel_CamToBase(5, 1) = 0;
		Tvel_CamToBase(5, 2) = 0;

		Tvel_CamToBase(5, 3) = -sin(theta_6) * (cos(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) + sin(7 * theta_2) * cos(theta_3) * sin(theta_4)) - cos(theta_6) * (cos(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3)
			+ cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) - sin(7 * theta_2) * cos(theta_3) * cos(theta_4)) + sin(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)));
		Tvel_CamToBase(5, 4) = cos(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)) - sin(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) 
			- sin(7 * theta_2) * cos(theta_3) * cos(theta_4));
		Tvel_CamToBase(5, 5) = cos(theta_6) * (cos(theta_4) * (sin(theta_3) * sin(theta_3) + cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) + sin(7 * theta_2) * cos(theta_3) * sin(theta_4)) - sin(theta_6) * (cos(theta_5) * (sin(theta_4) * (sin(theta_3) * sin(theta_3)
			+ cos(7 * theta_2) * cos(theta_3) * cos(theta_3)) - sin(7 * theta_2) * cos(theta_3) * cos(theta_4)) + sin(theta_5) * (cos(theta_3) * sin(theta_3) - cos(7 * theta_2) * cos(theta_3) * sin(theta_3)));

		return Tvel_CamToBase;

	}

	vector<double> kec_motors2End()//输入 theta_1 m_dj_1 m_dj_2 m_dj_3 m_dj_4 theta_4 theta_5 theta_6 输出T07(末端到基座标系位姿矩阵
	{
		double Line_1, Line_2, Line_3, Line_4, Line;
		Matrix4d T450, T451;

		Line_1 = Line + line_coef_for * m_dj_1 / 7;
		Line_2 = Line + line_coef_for * m_dj_2 / 7;
		Line_3 = Line + line_coef_back * m_dj_3 / 7;
		Line_4 = Line + line_coef_back * m_dj_4 / 7;

		if (Line_1 < Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			theta_3 = pi;
		}
		theta_2 = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);

		Matrix4d T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		/*T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;*/
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T_45 = T450 * T451;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	}

	vector<double> kec_joints2End()//输入 theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 输出T07(末端到基座标系位姿矩阵
	{
		Matrix4d T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07, T450, T451;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;



         Matrix4d u, A, B, C, D, E, F, G, H, T, E1, E2;		// Ed
		double a, b, h, X, Y, Z;
	    a = 0, b = 0;
		for (int i = 1; i < 8; i++) {
			a += 2 * 20 * cos(theta_2 / 2) * sin((theta_2 / 2) + (i - 1) * theta_2);
			b += 2 * 20 * cos(theta_2 / 2) * cos((theta_2 / 2) + (i - 1) * theta_2);
		}

		X = a * cos(theta_3);  Y = a * sin(theta_3); Z = b;
		//h = (238 - 49.07 / tan(pi / 2 - theta_1));
    //    if (log == 1 || log == 6) {
	// 		h = 238 - 49.07 / tan(pi / 2 - theta_1);
	// 	}
	// 	else if (log == 2 || log == 7) {
	// 		h = 238 - 49.07 / tan(pi / 2 - theta_1);
	// 	}
	// 	else {
	// 		h = 238 - 50.59 / tan(pi / 2 - theta_1);
	// 	}
		u << 1, 0, 0, 0,
			0, 1, 0, h,
			0, 0, 1, 0,
			0, 0, 0, 1;

		A << cos(theta_1), -sin(theta_1), 0, 0,
			sin(theta_1), cos(theta_1), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;

		B << 0, 0, 1, L1,
			0, -1, 0, 0,
			1, 0, 0, 0,
			0, 0, 0, 1;


		
		C << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;

		D << -sin(theta_4), -cos(theta_4), 0, 0,
			0, 0, -1, 0,
			cos(theta_4), -sin(theta_4), 0, L2,
			0, 0, 0, 1;
		E1 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		E2 << cos(theta_5), -sin(theta_5), 0, 0,
			sin(theta_5), cos(theta_5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		E = E1 * E2;

		F << cos(theta_6), -sin(theta_6), 0, 0,
			0, 0, -1, 0,
			sin(theta_6), cos(theta_6), 0, 0,
			0, 0, 0, 1;

		G << 1, 0, 0, 0,
			0, 0, -1, 0,
			0, 1, 0, L3,
			0, 0, 0, 1;

		// sunjian delete 2024-05-10 
		//Ed << -sin(m4) * Ldo, 0, cos(m4)* Ldo + L2, 1;

		/*H = u * A * B * C * D * E * F * G;*/
		H = A * B * C * D * E * F * G;
        T_07=H;
	//	std::cout<<"输出位置矩阵 H"<< H <<endl;
	//	std::cout<<"输出位置矩阵 T_07"<< T_07 <<endl;
		

/*
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0,
		 0, 0, 1, 0, 0, 0, 0, 1;


		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;

		T_34 << -sin(theta_4), -cos(theta_4), 0, 0,
		 0, 0, -1, 0, 
		 cos(theta_4), -sin(theta_4), 0, L2, 
		 0, 0, 0, 1;
		
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(theta_5), -sin(theta_5), 0, 0,
			sin(theta_5), cos(theta_5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;

		T_45 = T450 * T451;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 
		0, 0, -1, 0,
		 0, 1, 0, L3, 
		 0, 0, 0, 1;

  std::cout<<"输出关节角"<<theta_1<< " "<<theta_2<< " "<<theta_3<< " "<<theta_4<< " "<<theta_5<<endl;
    
		T_07 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;

		*/
 
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	}

	vector<double> kec_motors2End_addh(int log)//输入 theta_1 m_dj_1 m_dj_2 m_dj_3 m_dj_4 theta_4 theta_5 theta_6 h 输出末端位姿
	{
		double Line_1, Line_2, Line_3, Line_4, Line;

		Line_1 = Line + line_coef_for * m_dj_1 / 7;
		Line_2 = Line + line_coef_for * m_dj_2 / 7;
		Line_3 = Line + line_coef_back * m_dj_3 / 7;
		Line_4 = Line + line_coef_back * m_dj_4 / 7;

		if (Line_1 < Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			theta_3 = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			theta_3 = pi;
		}
		theta_2 = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);

		Matrix4d T_H0, T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07,T450,T451;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;
		if (log == 1 || log == 6) {
			h = 238 - 49.07 / tan(pi / 2 - theta_1);
		}
		else if (log == 2 || log == 7) {
			h = 238 - 52/ tan(pi / 2 - theta_1);
		}
		else {
			h = 238 - 50.59 / tan(pi / 2 - theta_1);
		}
		T_H0 << 1, 0, 0, 0, 0, 1, 0, h, 0, 0, 1, 0, 0, 0, 0, 1;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
		/*T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;*/
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(m5), -sin(m5), 0, 0,
			sin(m5), cos(m5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T_45 = T450 * T451;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;
		T_07 = T_H0 * T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	
	}

	vector<double> kec_joints2End_addh(int log)//输入 theta_1 theta_2 theta_3 theta_4 theta_5 theta_6 h 输出末端位姿
	{
		//std::cout<<"-------------------------特殊的正运动学----------------------------"<<log<<endl;
		Matrix4d T_H0, T_01, T_12, T_23, T_34, T_45, T_56, T_67, T_07,T450,T451;
		Matrix3d RotMatrix;
		MatrixXd moveDirection(3, 1), moveDirectionNew(3, 1);
		moveDirection << -1, 0, 0;


		if (log == 1 || log == 6) {
			h = 238 - 49.07 / tan(pi / 2 - theta_1);
		}
		else if (log == 2 || log == 7) {
			h = 238 - 52/ tan(pi / 2 - theta_1);
		}
		else {
			h = 238 - 50.59 / tan(pi / 2 - theta_1);
		}
		T_H0 << 1, 0, 0, 0, 0, 1, 0, h, 0, 0, 1, 0, 0, 0, 0, 1;
		T_01 << cos(theta_1), -sin(theta_1), 0, 0, sin(theta_1), cos(theta_1), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
		T_12 << 0, 0, 1, L1, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
		a = 0, b = 0;
		for (int i = 1; i < 8; i++)
		{
			a = a + 2 * L * cos((theta_2) / 2) * sin(((theta_2) / 2) + (i - 1) * (theta_2));
			b = b + 2 * L * cos((theta_2) / 2) * cos(((theta_2) / 2) + (i - 1) * (theta_2));
		}

		X = a * cos(theta_3); Y = a * sin(theta_3); Z = b;
		T_23 << cos(theta_3) * cos(theta_3) * cos(7 * theta_2) + sin(theta_3) * sin(theta_3), cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), cos(theta_3)* sin(7 * theta_2), X,
			cos(theta_3)* sin(theta_3)* cos(7 * theta_2) - cos(theta_3) * sin(theta_3), sin(theta_3)* sin(theta_3)* cos(7 * theta_2) + cos(theta_3) * cos(theta_3), sin(theta_3)* sin(7 * theta_2), Y,
			-cos(theta_3) * sin(7 * theta_2), -sin(theta_3) * sin(7 * theta_2), cos(7 * theta_2), Z,
			0, 0, 0, 1;
		T_34 << -sin(theta_4), -cos(theta_4), 0, 0, 0, 0, -1, 0, cos(theta_4), -sin(theta_4), 0, L2, 0, 0, 0, 1;
	/*	T_45 << cos(theta_5), -sin(theta_5), 0, 0, 0, 0, 1, 0, -sin(theta_5), -cos(theta_5), 0, 0, 0, 0, 0, 1;*/
		T450 << 1, 0, 0, Ldo,
			0, 0, 1, 0,
			0, -1, 0, 0,
			0, 0, 0, 1;
		T451 << cos(theta_5), -sin(theta_5), 0, 0,
			sin(theta_5), cos(theta_5), 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
		T_45 = T450 * T451;
		T_56 << cos(theta_6), -sin(theta_6), 0, 0, 0, 0, -1, 0, sin(theta_6), cos(theta_6), 0, 0, 0, 0, 0, 1;
		T_67 << 1, 0, 0, 0, 0, 0, -1, 0, 0, 1, 0, L3, 0, 0, 0, 1;



		T_07 = T_H0 * T_01 * T_12 * T_23 * T_34 * T_45 * T_56 * T_67;
		RotMatrix << T_07(0, 0), T_07(0, 1), T_07(0, 2), T_07(1, 0), T_07(1, 1), T_07(1, 2), T_07(2, 0), T_07(2, 1), T_07(2, 2);
		moveDirectionNew = RotMatrix * moveDirection;
		vector<double> positionPosture(9, 0);
		positionPosture[0] = T_07(0, 3);
		positionPosture[1] = T_07(1, 3);
		positionPosture[2] = T_07(2, 3);
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > -pi / 2 && atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < pi / 2)
		{
			positionPosture[3] = atan2(T_07(2, 1), T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(T_07(1, 0), T_07(0, 0));
		}
		if (atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) < -pi / 2 || atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))) > pi / 2)
		{
			positionPosture[3] = atan2(-T_07(2, 1), -T_07(2, 2)); positionPosture[4] = atan2(-T_07(2, 0), sqrt(pow(T_07(2, 1), 2) + pow(T_07(2, 2), 2))); positionPosture[5] = atan2(-T_07(1, 0), -T_07(0, 0));
		}
		positionPosture[6] = moveDirectionNew(0);
		positionPosture[7] = moveDirectionNew(1);
		positionPosture[8] = moveDirectionNew(2);
		return positionPosture;
	
	}

	//向逆运动学写入参数
	void Param_writeto_Inv_sol(vector<double> &cartesian_in){
		theta_x = cartesian_in[3];
        theta_y = cartesian_in[4];
        theta_z = cartesian_in[5];
        Px = cartesian_in[0];
        Py = cartesian_in[1];
        Pz = cartesian_in[2];
	}

	//更新关节角
	void Param_joints_update(vector<double> motor_angles_in){ //m_dj_1- m_dj_4为关机转角，m2为柔性偏摆角，m3为柔性自转角

		double m2_temp, m3_temp;

		Line_1 = Line + line_coef_for * motor_angles_in[4] / 7;//m_dj_1- m_dj_4/��װʱע����˳��
		Line_2 = Line + line_coef_for * motor_angles_in[5] / 7;
		Line_3 = Line + line_coef_back * motor_angles_in[6] / 7;
		Line_4 = Line + line_coef_back * motor_angles_in[7] / 7;

		if (Line_1 < Line_3)
		{
			m3_temp = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3));
		}
		else if (Line_1 > Line_3)
		{
			m3_temp = atan((Line_2 * Line_1 - Line_4 * Line_1 + Line_2 * Line_3 - Line_4 * Line_3) / (Line_2 * Line_1 + Line_4 * Line_1 - Line_2 * Line_3 - Line_4 * Line_3)) + pi;
		}
		else
		{
			m3_temp = pi;
		}
		m2_temp = 2 * asin((Line_1 + Line_3) / 4 * sqrt((Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 - Line_2 * Line_3 + Line_4 * Line_1 - Line_4 * Line_3))
			+ (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3) * (Line_2 * Line_1 + Line_2 * Line_3 - Line_4 * Line_1 - Line_4 * Line_3)) / ((Line_4 * Line_1 + Line_4 * Line_3 + Line_2 * Line_1 + Line_2 * Line_3) * R);

		theta_1 = motor_angles_in[0];
		theta_2 = m3_temp;
		theta_3 = m3_temp;
		theta_4 = motor_angles_in[3];
		theta_5 = motor_angles_in[4];
		theta_6 = motor_angles_in[5];
	}
    
	//更新关节角，仿真环境中使用
	void Param_joints_update_simulation(vector<double> joint_simulation_position_in)
	{
		theta_1 = joint_simulation_position_in[0];
		theta_2 = joint_simulation_position_in[1];
		theta_3 = joint_simulation_position_in[2];
		theta_4 = joint_simulation_position_in[3];
		theta_5 = joint_simulation_position_in[4];
		theta_6 = joint_simulation_position_in[5];
	}

		//更新关节角，给逆运动学用
	void Param_joints_update_simulation_for(vector<double> joint_simulation_position_for)
	{
		
		theta5_for= joint_simulation_position_for[4];
		theta6_for = joint_simulation_position_for[5];

		//cout<<"关节角5 6  "<<theta5_for<<"  "<<theta6_for<<endl;
	}


	//更新电机角
	void Param_motors_update(vector<double> motor_angles_in){ //m_dj_1- m_dj_4为关机转角，m2为柔性偏摆角，m3为柔性自转角
		theta_1 = motor_angles_in[0];
		m_dj_1 = motor_angles_in[6];
		m_dj_2 = motor_angles_in[7];
		m_dj_3 = motor_angles_in[8];
		m_dj_4 = motor_angles_in[9];
		theta_4 = motor_angles_in[3];
		theta_5 = motor_angles_in[4];
		theta_6 = motor_angles_in[5];
	}

	vector<double> joint2motorAngle(double m_1,double m_2,double m_3)
	{
		vector<double> m_dj(4);

		r = Ldd / tan(m_2 / 2);
		r1 = r - R * cos(m_3 - pi / 4);
		r2 = r - R * sin(m_3 - pi / 4);
		r3 = r + R * cos(m_3 - pi / 4);
		r4 = r + R * sin(m_3 - pi / 4);

		l1 = r1 * 2 * sin(m_2 / 2) * 7 - ((pi / 2) - m_1) * 10;
		l2 = r2 * 2 * sin(m_2 / 2) * 7 - ((pi / 2) - m_1) * 10;
		l3 = r3 * 2 * sin(m_2 / 2) * 7 - ((pi / 2) - m_1) * 10;
		l4 = r4 * 2 * sin(m_2 / 2) * 7 - ((pi / 2) - m_1) * 10;

		m_dj[0] = (l1 - l10) / line_coef_for;
		m_dj[1] = (l2 - l20) / line_coef_for;
		m_dj[2] = (l3 - l30) / line_coef_back;
		m_dj[3] = (l4 - l40) / line_coef_back;

		return m_dj;
	}

	vector<double> motorAngle2joint(double m1, double m_dj_1, double m_dj_2, double m_dj_3, double m_dj_4){

		l1 = m_dj_1 * line_coef_for + l10 +((pi / 2) - m1) * 10;
		l2 = m_dj_2 * line_coef_for + l20 +((pi / 2) - m1) * 10;
		l3 = m_dj_3 * line_coef_for + l30 +((pi / 2) - m1) * 10;
		l4 = m_dj_4 * line_coef_for + l40 +((pi / 2) - m1) * 10;

		if (l1 < l3)
		{
			m3 = atan((l2 * l1 - l4 * l1 + l2 * l3 - l4 * l3) / (l2 * l1 + l4 * l1 - l2 * l3 - l4 * l3));
		}
		else if (l1 > l3)
		{
			m3 = atan((l2 * l1 - l4 * l1 + l2 * l3 - l4 * l3) / (l2 * l1 + l4 * l1 - l2 * l3 - l4 * l3))+pi;
		}
		else if (l1 = l3)
		{
			m3 = pi;
		}
		m2 = 2 * asin((l1 + l3) /28 * sqrt((l1 * l2 - l2 * l3 + l1 * l4 - l3 * l4) * (l1 * l2 - l2 * l3 + l1 * l4 - l3 * l4) + (l1 * l2 + l2 * l3 - l1 * l4 - l3 * l4) * (l1 * l2 + l2 * l3 - l1 * l4 - l3 * l4))
			/ (l1 * l4 + l3 * l4 + l1 * l2 + l3 * l2) / R);

		vector<double> joint_m = {m2, m3+45*pi/180};
		return joint_m;
	}
};
